import cv2
import numpy as np
import datetime
from collections import deque
import rclpy

# 初始化变量
zero_count = 0
last_x0, last_y0 = 320, 240
points_queue = deque(maxlen=5)  # 存储连续三帧的有效结果

def process_frame(frame):


    # 默认值，防止未定义的情况
    x0, y0 = last_x0, last_y0

    # 定义存储点的数组，最多存储5个点的(x, y)坐标
    #points_arr = np.zeros((5, 2))

    # 定义感兴趣区域（ROI）
    ROI = frame[180:480, 0:640]
    gray = cv2.cvtColor(ROI, cv2.COLOR_BGR2GRAY)
    # 使用Canny边缘检测
    edges = cv2.Canny(ROI, 100, 250)
    #_, otsu_thresh = cv2.threshold(gray, 0, 255, cv2.THRESH_BINARY + cv2.THRESH_OTSU)
    # 查找边缘轮廓
    contours, _ = cv2.findContours(edges, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)

    # 创建两个空白图像用于填充
    ROI_1 = np.zeros_like(ROI)
    ROI_2 = np.zeros_like(ROI)

    if contours:
        for cnt in contours:
            cv2.fillPoly(ROI_1, [cnt], (0, 0, 255))
            #cv2.drawContours(ROI_1, [cnt], -1, (0, 255, 255), 2)
    #cv2.imshow('ROI_1', ROI_1)
    # 转换为HSV颜色空间
    HSV = cv2.cvtColor(ROI, cv2.COLOR_BGR2HSV)

    # 定义颜色阈值
    lower_threshold = np.array([0, 0, 0])
    upper_threshold = np.array([80, 100, 100])
    # 创建颜色掩膜
    mask = cv2.inRange(HSV, lower_threshold, upper_threshold)
    #inverse_mask = cv2.bitwise_not(mask)
    kernel1 = np.ones((5, 5), np.uint8)
    mask = cv2.morphologyEx(mask, cv2.MORPH_OPEN, kernel1, iterations=1)
    ROI_2[mask == 255] = [0, 255, 0]
    #cv2.imshow('ROI_2', ROI_2)
    #cv2.imshow('inverse_mask', inverse_mask)
    # 创建红色掩膜和绿色掩膜
    red_mask = cv2.inRange(ROI_1, np.array([0, 0, 255]), np.array([0, 0, 255]))
    green_mask = cv2.inRange(ROI_2, np.array([0, 255, 0]), np.array([0, 255, 0]))

    # 逻辑AND操作找到交集
    intersection_mask = cv2.bitwise_and(red_mask, green_mask)
    intersection_mask = cv2.morphologyEx(intersection_mask, cv2.MORPH_OPEN, kernel1, iterations=1)
    #cv2.imshow('intersection_mask', intersection_mask)
    # 查找重叠掩膜中的轮廓
    contours, _ = cv2.findContours(intersection_mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE)

    count = 0
    points = []
    if contours:
        for contour in contours:
            Area = cv2.contourArea(contour)
            if Area >= 100:
                M = cv2.moments(contour)
                if M["m00"] != 0:
                    cX = int(M["m10"] / M["m00"])
                    cY = int(M["m01"] / M["m00"]) + 180
                else:
                    cX, cY = 0, 180
                points.append((cX, cY))  # 添加有效轨迹块点
                count += 1

    if count > 0:
        # 获取这一帧的y最小的点并添加到队列中
        min_y_point = min(points, key=lambda p: p[1])
        points_queue.append(min_y_point)
        zero_count = 0  # 重置丢线计数
    else:
        zero_count += 1
        if zero_count >= 10 and points_queue:
            # 当连续丢线，使用上一次有效点的对称值
            x0, y0 = 320 - (last_x0 - 320), last_y0
            zero_count = 0
        else:
            x0, y0 = last_x0, last_y0

    if len(points_queue) == 3:
        # 在三帧中找到y最小的点
        x0, y0 = min(points_queue, key=lambda p: p[1])
        '''
        获取三帧平均值
        avg_x0 = np.mean([p[0] for p in points_queue])
        avg_y0 = np.mean([p[1] for p in points_queue])
        x0, y0 = int(avg_x0), int(avg_y0)
        '''
        last_x0, last_y0 = x0, y0  # 更新最后有效点
        points_queue.popleft()    #移除最早的一帧

    # 绘制结果点
    #cv2.circle(frame, (x0, y0), 5, (0, 255, 0), 3)
    return x0,y0

  